#include "Process.h"
using namespace std;
using namespace cv;


void NormalizePoint(std::vector<cv::Point2f>& Src, std::vector<cv::Point2f>* Dis, float& xScale, float& yScale, float& xMean, float& yMean)
{
	if (true)
	{
		float xMax = 0;
		float xMin = FLT_MAX;
		float yMax = 0;
		float yMin = FLT_MAX;
		float xSum = 0;
		float ySum = 0;
		for (size_t i = 0; i < Src.size(); i++)
		{
			//if (Src[i].x> xMax)
			//{
			//	xMax = Src[i].x;
			//}
			//if (Src[i].x < xMin)
			//{
			//	xMin = Src[i].x;
			//}
			//if (Src[i].y > yMax)
			//{
			//	yMax = Src[i].y;
			//}
			//if (Src[i].y < yMin)
			//{
			//	yMin = Src[i].y;
			//}
			xSum += Src[i].x;
			ySum += Src[i].y;
		}
	
		xScale = 1 / ((xMax - xMin) * 0.5);
		yScale = 1 / ((yMax - yMin) * 0.5);
		xMean = xSum / Src.size();
		yMean = ySum / Src.size();

		Dis->resize(Src.size());
		for (size_t i = 0; i < Src.size(); i++)
		{
			(*Dis)[i].x = (Src[i].x - xMean);
			(*Dis)[i].y = (Src[i].y - yMean);
		}	
	}
}


void GetDesignMatrix(std::vector<cv::Point2f>& Plist, Mat& MatrixD1, Mat& MatrixD2)
{
	int length = Plist.size();
	MatrixD1 = Mat(length, 3, CV_32FC1);
	MatrixD2 = Mat(length, 3, CV_32FC1);
	float x, y;
	for (size_t i = 0; i < Plist.size(); i++)
	{
		x = Plist[i].x;
		y = Plist[i].y;

		MatrixD1.at<float>(i, 0) = x * x;
		MatrixD1.at<float>(i, 1) = x * y;
		MatrixD1.at<float>(i, 2) = y * y;

		MatrixD2.at<float>(i, 0) = x;
		MatrixD2.at<float>(i, 1) = y;
		MatrixD2.at<float>(i, 2) = 1;
	}
}


void myEigen(Mat& A, Mat& D, Mat& V)
{
	eigen(A, D, V);
	Mat E = Mat::eye(A.size(), A.type());
	for (int i = 0; i < A.rows; i++)
	{
		E.at<double>(i, i) = D.at<double>(0, i);
	}
	D = E.clone();

}


void DirectFitEllipse(std::vector<cv::Point2f>& PointList,cv::RotatedRect* Ellipse)
{
	float xScale, yScale, xMean, yMean;
	std::vector<cv::Point2f> P;
	NormalizePoint(PointList, &P, xScale, yScale, xMean, yMean);
	std::vector<std::vector<float>> Matrix;

	Mat MatrixD1, MatrixD2;
	GetDesignMatrix(P, MatrixD1, MatrixD2);

	Mat S1 = MatrixD1.t() * MatrixD1;
	Mat S2 = MatrixD1.t() * MatrixD2;
	Mat S3 = MatrixD2.t() * MatrixD2;

	


	Mat T = -S3.inv() * S2.t();
	Mat M = S1 + S2 * T;

	Mat cl = M.clone();
	for (size_t i = 0; i < M.cols; i++)
	{
		M.at<float>(0, i) = cl.at<float>(2, i) / 2;
		M.at<float>(1, i) = -M.at<float>(1, i);
		M.at<float>(2, i) = cl.at<float>(0, i) / 2;
	}

	M.convertTo(M,CV_64FC1);

	Mat evec, eval;
	//Mat AA = (Mat_<double>(3, 3) <<
	//	-4493, 0, 4044,
	//	0, 0, 0,
	//	4993, 0, -4993);
	//Mat D, V;
	//myEigen(AA, D, V);
	//Mat A_r = V.t() * D * V;
	eigen(M, evec, eval);
	Mat RTA,RTB;
	cv::multiply(4 * eval(Rect(0, 0, 3, 1)), eval(Rect(0, 2, 3, 1)), RTA);
	cv::multiply(eval(Rect(0, 1, 3, 1)), eval(Rect(0, 1, 3, 1)), RTB);

	Mat cond = RTA - RTB;
	int index = -1;
	for (size_t i = 0; i < 3; i++)
	{
		if (cond.at<float>(0,i)>0&&!isinf(cond.at<float>(0, i)))
		{
			index = i;
		}
	}
	
	if (index<0)
	{
		return;
	}

	Mat A1H = eval(Rect(index, 0, 1, 3));
	Mat A1L = T * A1H;


	float a, b, c, d, e, f;

	//ellip.a = tA * sy * sy;
	//ellip.b = tB * sx * sy;
	//ellip.c = tC * sx * sx;
	//ellip.d = -2 * tA * sy * sy * mx - tB * sx * sy * my + tD * sx * sy * sy;
	//ellip.e = -tB * sx * sy * mx - 2 * tC * sx * sx * my + tE * sx * sx * sy;
	//ellip.f = tA * sy * sy * mx * mx + tB * sx * sy * mx * my + tC * sx * sx * my * my
	//	- tD * sx * sy * sy * mx - tE * sx * sx * sy * my + tF * sx * sx * sy * sy;

	xScale = 1;
	yScale = 1;

	a = A1H.at<float>(0, 0);
	b = A1H.at<float>(1, 0);
	c = A1H.at<float>(2, 0);
	d = A1L.at<float>(0, 0) - 2 * a * xMean - b * yMean;
	e = A1L.at<float>(1, 0)  - 2 * c * yMean  - b * xMean;
	f = A1L.at<float>(2, 0) 
		+ a * xMean * xMean 
		+ c * yMean * yMean 
		+ b * xMean * yMean 
		- d * xMean 
		- e * yMean;

	//ax ^ 2 + bxy + cy ^ 2 + dx + ey + f = 0

	//x^2 + y^2=100;


	Mat Nc(6,1,CV_32FC1);

	Nc.at<float>(0, 0) = a;
	Nc.at<float>(1, 0) = b;
	Nc.at<float>(2, 0) = c;
	Nc.at<float>(3, 0) = d;
	Nc.at<float>(4, 0) = e;
	Nc.at<float>(5, 0) = f;


	Nc = Nc / norm(Nc);

	a = Nc.at<float>(0, 0);
	b = Nc.at<float>(1, 0);
	c = Nc.at<float>(2, 0);
	d = Nc.at<float>(3, 0);
	e = Nc.at<float>(4, 0);
	f = Nc.at<float>(5, 0);


	double tmp1 = b * b - 4 * a * c;
	double tmp2 = sqrt((a - c) * (a - c) + b * b);
	double tmp3 = a * e * e + c * d * d - b * d * e + tmp1 * f;

	double r1 = -sqrt(2 * tmp3 * (a + c + tmp2)) / tmp1;
	double r2 = -sqrt(2 * tmp3 * (a + c - tmp2)) / tmp1;
	float rl = r1 >= r2 ? r1 : r2;
	float rs = r1 <= r2 ? r1 : r2;

	float cx = (2 * c * d - b * e) / tmp1;
	float cy = (2 * a * e - b * d) / tmp1;

	float phi = 0.5 * atan2(b, a - c);
	if (r1 > r2)
		phi += 1.57079632679489661923;


	*Ellipse = RotatedRect(Point2f(cx, cy), Size(rl, rs), phi);
}





